import math
import numpy
import platform
import numpy as np
from ctypes import *
from decimal import *
from config import settings
import scipy.linalg as linalg
from models.custom_class import *

gCallbackFuncList = list()  # 定义全局列表接收回调函数，防止系统回收
# 雷达参数设置
RunMode = settings.RADAR.RUNMODE
Speed = settings.RADAR.SPEED
AngleSceneScan = settings.RADAR.ANGLESCENESCAN

dllPath = settings.STATIC_DIR + '/sdk/CDPSIMCLIENT-win64.dll'
soPath = settings.STATIC_DIR + '/sdk/cdpsimclient-linux64.so'
if platform.system() == 'Windows':
    from ctypes import windll

    dll = windll.LoadLibrary(dllPath)
    CALLBACK = WINFUNCTYPE(None, c_uint, c_int, POINTER(c_char), c_int)
else:
    from ctypes import cdll

    dll = cdll.LoadLibrary(soPath)
    CALLBACK = CFUNCTYPE(None, c_uint, c_int, POINTER(c_char), c_int)


def set_callback_function(func, yard_id):
    callback_func = CALLBACK(func)
    gCallbackFuncList.append(callback_func)
    dll.NET_SDK_SIMCLT_Set_Callback(callback_func, yard_id)


def bytes_cloud_frame_rotated(kwargs: dict):
    ws_id = kwargs['ws_id']
    websocket = get_websocket_by_wsid(ws_id=ws_id)
    coal_yard = websocket.coalYard

    points_data = kwargs['data']
    cloud_ndarray: numpy.ndarray = np.frombuffer(points_data, dtype=np.int16).reshape(-1, 3)

    cid = kwargs['cid']
    radars = coal_yard.coalRadarList
    for radar in radars:
        if radar.id == cid:
            div = np.array([100, 100, 100])
            # div = np.array([1, 1, 1])
            radar_cloud_ndarray = np.divide(cloud_ndarray, div)
            # radar_cloud_ndarray = radar_cloud_ndarray.astype(np.float16)
            rotated_radar_cloud_ndarray: numpy.ndarray = euler_rotate(radar_cloud_ndarray, radar)
            new_cloud_list = rotated_radar_cloud_ndarray.tolist()

            return new_cloud_list


def euler_rotate(cloud_ndarray: numpy.ndarray, radar: CoalRadar):
    axis_x = [1, 0, 0]
    axis_y = [0, 1, 0]
    axis_z = [0, 0, 1]
    # 旋转角度
    x_radian = radar.rotateX * math.pi / 180
    y_radian = radar.rotateY * math.pi / 180
    z_radian = radar.rotateZ * math.pi / 180
    # 旋转矩阵常量
    x_matrix = linalg.expm(np.cross(np.eye(3), axis_x / linalg.norm(axis_x) * x_radian))
    y_matrix = linalg.expm(np.cross(np.eye(3), axis_y / linalg.norm(axis_y) * y_radian))
    z_matrix = linalg.expm(np.cross(np.eye(3), axis_z / linalg.norm(axis_z) * z_radian))

    # 点云旋转操作
    matrix = np.dot(x_matrix, np.dot(y_matrix, z_matrix))
    rotate_cloud_array = np.dot(cloud_ndarray, matrix)

    s = Decimal('100')
    # 点云平移操作
    shift_xyz = np.array(
        [[float(Decimal(radar.shiftX) / s), float(Decimal(radar.shiftY) / s), float(Decimal(radar.shiftZ) / s)]])
    new_cloud_array = rotate_cloud_array + shift_xyz

    # new_cloud_array.astype(np.float16)
    return new_cloud_array


def radars_start_connect(radars: List[CoalRadar]):
    for radar in radars:
        radar_connect(radar)


def radar_connect(radar: CoalRadar):
    cid = radar.id
    ip = bytes(radar.ip, encoding='utf-8')
    port = radar.port
    dll.NET_SDK_SIMCLT_StartConnect(cid, ip, port, 120)
    # print('雷达已连接， cid ==', cid)


# 输入websocket的id值，返回一个 websocket 对象
def get_websocket_by_wsid(ws_id: int):
    websocket = cast(ws_id, py_object).value
    return websocket


def radar_stop(c_id):
    dll.NET_SDK_SIMCLT_ZTRD_RotateStop(c_id)
    dll.NET_SDK_SIMCLT_StopConnectCid(c_id)

